#include "CoordinateCal.h"
#include <stdlib.h>
#include <stdio.h>

/**
 * @brief 高斯消元法求解多元一次方程组，
 * @param matrix 输入的增广矩阵,矩阵形状为N*(N+1)
 * @param N 矩阵的行数
 * @param ret 输出的解
 */
static void gaussianElimination(double *matrix, int N, double *ret)
{
#define getMatrix(i, j) matrix[i * (N +1) + j]
    // 转换为上三角矩阵
    for (int i = 0; i < N; i++)
    {
        // 寻找当前列的最大值
        double maxEl = getMatrix(i, i);
        int maxRow = i;
        for (int k = i + 1; k < N; k++)
        {
            if (getMatrix(k, i) > maxEl)
            {
                maxEl = getMatrix(k, i);
                maxRow = k;
            }
        }
        // 交换最大行和当前行
        for (int k = i; k < N + 1; k++)
        {
            double tmp = getMatrix(maxRow, k);
            getMatrix(maxRow, k) = getMatrix(i, k);
            getMatrix(i, k) = tmp;
        }
        // 归一化当前行
        for (int k = i + 1; k < N; k++)
        {
            double c = -getMatrix(k, i) / getMatrix(i, i);
            for (int j = i; j < N + 1; j++)
            {
                if (i == j)
                {
                    getMatrix(k, j) = 0;
                }
                else
                {
                    getMatrix(k, j) += c * getMatrix(i, j);
                }
            }
        }
    }
    // 回代求解
    for (int i = N - 1; i >= 0; i--)
    {
        ret[i] = getMatrix(i, N) / getMatrix(i, i);
        for (int k = i - 1; k >= 0; k--)
        {
            getMatrix(k, N) -= getMatrix(k, i) * ret[i];
        }
    }
#undef getMatrix
}

double Zadd(double a, double b)
{
    return a + b;
}

double Zsub(double a, double b)
{
    return a - b;
}

double Zmul(double a, double b)
{
    return a * b;
}

double Zdiv(double a, double b)
{
    return a / b;
}

double Znone(double a, double b)
{
    return a;
}

/**
 * @brief 计算数组的和
 * @param a 数组a
 * @param b 数组b
 * @param n 数组长度
 * @param code 对两个数组的操作，'+'为求和，'-'为求差
 */
static double sum(double *a, double *b, int n, char code)
{
    double _tmp = 0;
    double (*tmpf)(double, double);
    // 选择操作
    switch (code)
    {
    case '+':
        tmpf = Zadd;
        break;
    case '-':
        tmpf = Zsub;
        break;
    case '*':
        tmpf = Zmul;
        break;
    case '/':
        tmpf = Zdiv;
        break;
    default:
        tmpf = Znone;
        break;
    }
    for (int i = 0; i < n; i++)
    {
        _tmp += tmpf(a[i], b[i]);
    }
    return _tmp;
}

/**
 * @brief 计算变换矩阵
 * @param pointlist 输入的一组数据，包含原始向量和映射后向量
 * @param Dim 输入的数据维度，及pointlist中src和dst的向量长度
 * @param pointN 输入的坐标点个数
 * @param A 输出的变换矩阵，需要有足够的空间存储，矩阵形状为(Dim+1)*(Dim+1)
 * @return void
 */
DLL_EXPORT void CoordinateCal(point *pointlist, int Dim, int pointN, double *A)
{
    // 申请内存，
    double *srcdata = (double *)malloc(sizeof(double) * 2 * Dim * pointN); // 原始数据,用来重新排列数据
    // 重新排列数据
    for (int i = 0; i < pointN; i++)
    {
        for (int j = 0; j < Dim; j++)
        {
            srcdata[(j * pointN + i)] = pointlist[i].src[j];
            srcdata[(j * pointN + i) + pointN * Dim] = pointlist[i].dst[j];
        }
    }
#ifdef DEBUG
    for (int i = 0; i < (2 * Dim * pointN); i++)
    {
        if (i % pointN == 0)
        {
            printf("\n");
        }
        printf("%lf\t", srcdata[i]);
    }
    printf("\n");
#endif
    /**
     * srcdata的形状为Dim*pointN*2
     * |X11 X12 ... X1n |
     * |X21 X22 ... X2n |
     * |... ... ... ... |
     * |Xm1 Xm2 ... Xmn |
     * |Y11 Y12 ... Y1n |
     * |Y21 Y22 ... Y2n |
     * |... ... ... ... |
     * |Ym1 Ym2 ... Ymn |
     */
    // 计算必要值
    double *X = (double *)malloc(sizeof(double) * (Dim + 1) * (Dim + Dim + 1)); // 将会用到的参数
    for (int i = 0; i < Dim; i++)
    {
        for (int j = 0; j < Dim + Dim; j++)
        {
            X[i * (Dim + Dim + 1) + j] = sum(srcdata + i * pointN, srcdata + j * pointN, pointN, '*');
        }
    }
    // 计算最后一列
    for (int i = 0; i < Dim; i++)
    {
        X[i * (Dim + Dim + 1) + Dim + Dim] = sum(srcdata + i * pointN, srcdata + i * pointN, pointN, 'n');
    }
    // 计算最后一行
    for (int i = 0; i < Dim + Dim; i++)
    {
        X[Dim * (Dim + Dim + 1) + i] = sum(srcdata + i * pointN, srcdata + i * pointN, pointN, 'n');
    }
    X[Dim * (Dim + Dim + 1) + Dim + Dim] = pointN;
#ifdef DEBUG
    for (int i = 0; i < (Dim + 1) * (Dim + Dim + 1); i++)
    {
        if (i % (Dim + Dim + 1) == 0)
        {
            printf("\n");
        }
        printf("%lf\t", X[i]);
    }
    printf("\n");
#endif
    /***
     * X的形状为(Dim+1)*(Dim+Dim+1)
     * |X0|X1|X2|...|Xn|Y1|Y2|...|Yn|1|
     * |X1|X1X1 X1X2 ... X1Y1 X1Y2 ... X1Yn X1|
     * |X2|X2X1 X2X2 ... X2Y1 X2Y2 ... X2Yn X2|
     * |...|... ... ... ... ... ... ... ... ...|
     * |Xn|XnX1 XnX2 ... XnY1 XnY2 ... XnYn Xn|
     * |1|X1 X2 ... Y1 Y2 ... Yn n|
     */
    // 构建增广矩阵
    double *matrix = (double *)malloc(sizeof(double) * (Dim + 1) * (Dim + 2));
    for (int i = 0; i < Dim; i++) // 总共需要求解i次
    {
        // 每次求解都需要重新构建增广矩阵
        for (int j = 0; j < Dim + 1; j++)
        {
            // 构建增广矩阵
            // 前Dim列
            for (int k = 0; k < Dim; k++)
            {
                matrix[j * (Dim + 2) + k] = X[j * (Dim + Dim + 1) + k];
            }
            // 倒数第二列
            matrix[j * (Dim + 2) + Dim] = X[(j + 1) * (Dim + Dim + 1) - 1];
            // 最后一列
            matrix[j * (Dim + 2) + Dim + 1] = X[j * (Dim + Dim + 1) + i + Dim];
        }
// 求解
#ifdef DEBUG
        printf("matrix:\n");
        for (int i = 0; i < (Dim + 1) * (Dim + 2); i++)
        {
            if (i % (Dim + 2) == 0)
            {
                printf("\n");
            }
            printf("%lf\t", matrix[i]);
        }
        printf("\n");
#endif
        gaussianElimination(matrix, Dim + 1, A + i * (Dim + 1));
    }
    // 最后一行全零
    for (int i = 0; i < Dim + 1; i++)
    {
        A[Dim * (Dim + 1) + i] = 0;
    }
    A[Dim * (Dim + 1) + Dim] = 1;
    // 释放内存
    free(srcdata);
    free(X);
    free(matrix);
}